Debugging IK

Now that you have calculated your joint angles given the end-effector pose, you can test your implementation quickly with the help of the IK_debug.py script that can be found in the project repo, here. We'll go over some of the important details of this script for you to focus on.

Test Cases

In the debugging script, we have provided you with some test cases against which you can cross-check your implemented IK code.

test_cases = {1:[[[2.16135,-1.42635,1.55109],
                  [0.708611,0.186356,-0.157931,0.661967]],
                  [1.89451,-1.44302,1.69366],
                  [-0.65,0.45,-0.36,0.95,0.79,0.49]],
              2:[[[-0.56754,0.93663,3.0038],
                  [0.62073, 0.48318,0.38759,0.480629]],
                  [-0.638,0.64198,2.9988],
                  [-0.79,-0.11,-2.33,1.94,1.14,-3.68]],
              3:[[[-1.3863,0.02074,0.90986],
                  [0.01735,-0.2179,0.9025,0.371016]],
                  [-1.1669,-0.17989,0.85137],
                  [-2.99,-0.12,0.94,4.06,1.29,-4.12]],
              4:[],
              5:[]}

The format for each case is structured as:

[[[EE position], [EE orientation as quaternions]], [WC location], [Joint angles]]

Where,

EE is the end-effector,

WC is the wrist-center, and

Joint angles are thetas 1 to 6 in radians.

You can generate additional test cases by launching the forward kinematics demo, as explained in the Debugging Forward Kinematics section. Use the sliders for the joint_state_publisher to set a specific set of joint angles, and obtain the corresponding EE position and orientation, as shown below:

IK Code

The test_code() function takes a specific test case as the input, and prints out the corresponding errors between the provided test values and the calculated values obtained from your inverse kinematics code. Once you add your IK code (along with the associated FK code) and calculate the necessary joint angles, you can replace the calculated wrist center position in the provided variable.

## Insert IK code here!

theta1 = 0
theta2 = 0
theta3 = 0
theta4 = 0
theta5 = 0
theta6 = 0

## 

## For error analysis please set the following variables of your WC location and EE location in the format of [x,y,z]
your_wc = [1,1,1] # <--- Load your calculated WC values in this array

The rest of the code will utilize this wrist center position, and the thetas to calculate the corresponding errors. Using these error values as a basis, you can guage how well your current IK performs.

Optimization

While working on this project, you would have noticed that it can take a significant amount of time for the simulation to finish, even for any particular motion of picking up a single object and dropping it. There are certain approaches by which you can improve upon the total simulation time.

Sympy Calculations

In the current IK_server.py script, you will notice how most of the code that involves sympy calculations is outside the main for loop. Sympy can take up a lot of time to multiply matrices, and as a result, the forward kinematics part of your implementation can slow things down. So, in order to ensure that doesn't happen, it's best to follow the current structure and have your FK implementation outside the for loop. Here are a few more suggestions that we recommend:

• For your IK implementation, there might be certain matrix multiplications that are not required to be run for every received pose, and can thus be handled outside the loop. For example, obtaining the generic rotation matrix about any axis.
• Outside of debugging purposes, you need not require the simplify function. That can sometimes contribute to increased computations and you can consider not using it.
• A useful trick that helps a lot is to substitute any symbolic variables, like rotation angles, with their corresponding values before you multiply any matrices, if possible.

VMWare

A Virtual Machine tends to be limited as compared to a native setup when it comes to computational ability. The best possible way to minimize this gap is to ensure that you allocate as many resources as you possibly can. Increasing the memory (RAM), the number of processors and the graphics memory can often help bridge the gap.